/*
The MIT License
Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE

This file is part of embeddedRTPS.

Author: i11 - Embedded Software, RWTH Aachen University
*/

#include "rtps/messages/MessageTypes.h"
#include <stdio.h>


const unsigned char SCHEME_CDR_LE[2] = {0x00, 0x01};
const unsigned char SCHEME_PL_CDR_LE[2] = {0x00, 0x03};
const unsigned char RTPS_PROTOCOL_NAME[4] = {'R', 'T', 'P', 'S'};

void doCopyAndMoveOn(uint8_t *dst, const uint8_t **src, size_t size)
{
  memcpy(dst, *src, size);
  *src += size;
}

bool deserializeMessageHeader(struct MessageProcessingInfo *info,
                              struct Header *header) 
{
  if (getRemainingSize(info) < HeaderGetRawSize()) {
    return false;
  }

  const uint8_t *currentPos = getPointerToCurrentPos(info);
  doCopyAndMoveOn(header->protocolName, &currentPos, 4);
  doCopyAndMoveOn((uint8_t *)(&(header->protocolVersion)),
                  &currentPos, sizeof(struct ProtocolVersion_t));
  doCopyAndMoveOn(header->vendorId.vendorId, &currentPos,
                  sizeof(header->vendorId.vendorId));
  doCopyAndMoveOn(header->guidPrefix.id, &currentPos,
                  sizeof(header->guidPrefix.id));
  return true;
}

bool deserializeMessageSubmessageHeader(struct MessageProcessingInfo *info,
                              struct SubmessageHeader *header)
{
  if (getRemainingSize(info) < SubmessageHeaderGetRawSize()) {
    return false;
  }

  const uint8_t *currentPos = getPointerToCurrentPos(info);
  header->submessageId = (*currentPos++);
  header->flags = *(currentPos++);
  doCopyAndMoveOn((uint8_t *)(&(header->octetsToNextHeader)),
                  &currentPos, sizeof(uint16_t));
  return true;
}

bool deserializeMessageSubmessageData(struct MessageProcessingInfo *info,
                              struct SubmessageData *msg)
{
  if (getRemainingSize(info) < SubmessageHeaderGetRawSize()) {
    return false;
  }
  if (!deserializeMessageSubmessageHeader(info, &(msg->header))) {
    return false;
  }

  // Check for length including data
  if (getRemainingSize(info) <
      SubmessageHeaderGetRawSize() + msg->header.octetsToNextHeader) {
    return false;
  }

  const uint8_t *currentPos =
      getPointerToCurrentPos(info) + SubmessageHeaderGetRawSize();

  doCopyAndMoveOn((uint8_t *)(&(msg->extraFlags)), &currentPos,
                  sizeof(uint16_t));
  doCopyAndMoveOn((uint8_t *)(&(msg->octetsToInlineQos)),
                  &currentPos, sizeof(uint16_t));
  doCopyAndMoveOn(msg->readerId.entityKey, &currentPos,
                  sizeof(msg->readerId.entityKey));
  msg->readerId.entityKind = (*currentPos++);
  doCopyAndMoveOn(msg->writerId.entityKey, &currentPos,
                  sizeof(msg->writerId.entityKey));
  msg->writerId.entityKind = (*currentPos++);
  doCopyAndMoveOn((uint8_t *)(&(msg->writerSN.high)), &currentPos,
                  sizeof(msg->writerSN.high));
  doCopyAndMoveOn((uint8_t *)(&(msg->writerSN.low)), &currentPos,
                  sizeof(msg->writerSN.low));
  return true;
}

bool deserializeMessageSubmessageHeartbeat(struct MessageProcessingInfo *info,
                              struct SubmessageHeartbeat *msg)
{
  if (getRemainingSize(info) < SubmessageHeartbeatGetRawSize()){
    return false;
  }
  if (!deserializeMessageSubmessageHeader(info, &(msg->header))) {
    return false;
  }

  const uint8_t *currentPos =
      getPointerToCurrentPos(info) + SubmessageHeaderGetRawSize();

  doCopyAndMoveOn(msg->readerId.entityKey, &currentPos,
                  sizeof(msg->readerId.entityKey));
  msg->readerId.entityKind = (*currentPos++);
  doCopyAndMoveOn(msg->writerId.entityKey, &currentPos,
                  sizeof(msg->writerId.entityKey));
  msg->writerId.entityKind = (*currentPos++);
  doCopyAndMoveOn((uint8_t *)(&(msg->firstSN.high)), &currentPos,
                  sizeof(msg->firstSN.high));
  doCopyAndMoveOn((uint8_t *)(&(msg->firstSN.low)), &currentPos,
                  sizeof(msg->firstSN.low));
  doCopyAndMoveOn((uint8_t *)(&(msg->lastSN.high)), &currentPos,
                  sizeof(msg->lastSN.high));
  doCopyAndMoveOn((uint8_t *)(&(msg->lastSN.low)), &currentPos,
                  sizeof(msg->lastSN.low));
  doCopyAndMoveOn((uint8_t *)(&(msg->count.value)), &currentPos,
                  sizeof(msg->count.value));
  return true;
}

bool deserializeMessageSubmessageAckNack(struct MessageProcessingInfo *info,
                              struct SubmessageAckNack *msg)
{
  DataSize_t remainingSizeAtBeginning = getRemainingSize(info);
  if (remainingSizeAtBeginning < getRawSizeWithoutSNSet()) { // Size of SequenceNumberSet unknown
    return false;
  }
  if (!deserializeMessageSubmessageHeader(info, &(msg->header))) {
    return false;
  }

  const uint8_t *currentPos =
      getPointerToCurrentPos(info) + SubmessageHeaderGetRawSize();

  doCopyAndMoveOn(msg->readerId.entityKey, &currentPos,
                  sizeof(msg->readerId.entityKey));
  msg->readerId.entityKind = (*currentPos++);
  doCopyAndMoveOn(msg->writerId.entityKey, &currentPos,
                  sizeof(msg->writerId.entityKey));
  msg->writerId.entityKind = (*currentPos++);
  doCopyAndMoveOn((uint8_t *)(&(msg->readerSNState.base.high)),
                  &currentPos, sizeof(msg->readerSNState.base.high));
  doCopyAndMoveOn((uint8_t *)(&(msg->readerSNState.base.low)),
                  &currentPos, sizeof(msg->readerSNState.base.low));
  doCopyAndMoveOn((uint8_t *)(&(msg->readerSNState.numBits)),
                  &currentPos, sizeof(uint32_t));

  // Now we can check for full size
  if (remainingSizeAtBeginning <
      SubmessageAckNackGetRawSize(msg->readerSNState)) {
    return false;
  }

  if (msg->readerSNState.numBits != 0) {
    if (4 * ((msg->readerSNState.numBits / 32) + 1) >
        sizeof(msg->readerSNState.bitMap)) {
      while (1)
        ;
    }
    doCopyAndMoveOn((uint8_t *)(msg->readerSNState.bitMap),
        &currentPos, 4 * ((msg->readerSNState.numBits / 32) + 1));
  }
  doCopyAndMoveOn((uint8_t *)(&(msg->count.value)), &currentPos,
                  sizeof(msg->count.value));
  return true;
}

bool serializeMessageHeader(struct rtps_PBufWrapper *buffer, struct Header *header)
{
  if (!rtps_PBufWrapper_reserve(buffer, HeaderGetRawSize())) {
    return false;
  }

  rtps_PBufWrapper_append(buffer, header->protocolName, 4);
  rtps_PBufWrapper_append(buffer, (const uint8_t*)(&(header->protocolVersion)),
                sizeof(ProtocolVersion_t));
  rtps_PBufWrapper_append(buffer, header->vendorId.vendorId, sizeof(VendorId_t));
  rtps_PBufWrapper_append(buffer, header->guidPrefix.id, sizeof(struct GuidPrefix_t));
  return true;
}

bool serializeMessageSubmessageHeader(struct rtps_PBufWrapper *buffer, struct SubmessageHeader header)
{
  if (!rtps_PBufWrapper_reserve(buffer, HeaderGetRawSize())) {
    return false;
  }
  rtps_PBufWrapper_reserve(buffer, SubmessageHeaderGetRawSize());

  rtps_PBufWrapper_append(buffer, (const uint8_t*)(&header.submessageId),
                sizeof(SubmessageKind));
  rtps_PBufWrapper_append(buffer, &header.flags, sizeof(uint8_t));
  rtps_PBufWrapper_append(buffer, (const uint8_t*)(&header.octetsToNextHeader),
                sizeof(uint16_t));
  return true;
}

/*
bool serializeMessageSubmessageInfoDST(Buffer *buffer, struct SubmessageInfoDST *msg)
{
  if (!buffer.reserve(SubmessageInfoDSTGetRawSize())) {
    return false;
  }

  bool ret = serializeMessageSubmessageHeader(buffer, msg.header);
  if (!ret) {
    return false;
  }

  return buffer.append(msg.guidPrefix.id.data(), sizeof(struct GuidPrefix_t));
}
*/
bool serializeMessageSubmessageData(struct rtps_PBufWrapper *buffer, struct SubmessageData *msg)
{
  if (!rtps_PBufWrapper_reserve(buffer, SubmessageDataGetRawSize())) {
    return false;
  }

  serializeMessageSubmessageHeader(buffer, msg->header);

  rtps_PBufWrapper_append(buffer,(const uint8_t *)(&(msg->extraFlags)), sizeof(uint16_t));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->octetsToInlineQos)), sizeof(uint16_t));
  rtps_PBufWrapper_append(buffer, msg->readerId.entityKey, sizeof(msg->readerId.entityKey));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->readerId.entityKind)), sizeof(EntityKind_t));
  rtps_PBufWrapper_append(buffer, msg->writerId.entityKey, sizeof(msg->writerId.entityKey));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->writerId.entityKind)), sizeof(EntityKind_t));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->writerSN.high)), sizeof(msg->writerSN.high));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->writerSN.low)), sizeof(msg->writerSN.low));
  return true;
}


bool serializeMessageSubmessageHeartbeat(struct rtps_PBufWrapper *buffer, struct SubmessageHeartbeat *msg)
{
  if (!rtps_PBufWrapper_reserve(buffer, SubmessageHeartbeatGetRawSize())) {
    return false;
  }

  serializeMessageSubmessageHeader(buffer, msg->header);

  rtps_PBufWrapper_append(buffer, msg->readerId.entityKey, sizeof(msg->readerId.entityKey));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->readerId.entityKind)),
                sizeof(EntityKind_t));
  rtps_PBufWrapper_append(buffer, msg->writerId.entityKey, sizeof(msg->writerId.entityKey));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->writerId.entityKind)),
                sizeof(EntityKind_t));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->firstSN.high)),
                sizeof(msg->firstSN.high));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->firstSN.low)),
                sizeof(msg->firstSN.low));
  rtps_PBufWrapper_append(buffer,(const uint8_t *)(&(msg->lastSN.high)),
                sizeof(msg->lastSN.high));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->lastSN.low)),
                sizeof(msg->lastSN.low));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg->count.value)),
                sizeof(msg->count.value));
  return true;
}

bool serializeMessageSubmessageAckNack(struct rtps_PBufWrapper *buffer, struct SubmessageAckNack msg)
{
  if (!rtps_PBufWrapper_reserve(buffer, SubmessageAckNackGetRawSize(msg.readerSNState))) {
    return false;
  }

  serializeMessageSubmessageHeader(buffer, msg.header);

  rtps_PBufWrapper_append(buffer, msg.readerId.entityKey, sizeof(msg.readerId.entityKey));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg.readerId.entityKind)),
                sizeof(EntityKind_t));
  rtps_PBufWrapper_append(buffer, msg.writerId.entityKey, sizeof(msg.writerId.entityKey));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg.writerId.entityKind)),
                sizeof(EntityKind_t));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg.readerSNState.base.high)),
                sizeof(msg.readerSNState.base.high));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg.readerSNState.base.low)),
                sizeof(msg.readerSNState.base.low));
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg.readerSNState.numBits)),
                sizeof(uint32_t));
  if (msg.readerSNState.numBits != 0) {
    rtps_PBufWrapper_append(buffer, (const uint8_t *)(msg.readerSNState.bitMap),
                  4 * ((msg.readerSNState.numBits / 32) + 1));
  }
  rtps_PBufWrapper_append(buffer, (const uint8_t *)(&(msg.count.value)),
                sizeof(msg.count.value));
  return true;
}


uint16_t HeaderGetRawSize(void)
{
    return 4 + sizeof(struct ProtocolVersion_t) +
           sizeof(struct VendorId_t) + sizeof(struct GuidPrefix_t);
}

uint16_t SubmessageHeaderGetRawSize(void)
{
  return sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint16_t);
}

uint16_t SubmessageDataGetRawSize(void)
{
  return SubmessageHeaderGetRawSize() + sizeof(uint16_t) +
          sizeof(uint16_t) + (2 * 3 + 2 * 1) // EntityID
          + sizeof(struct SequenceNumber_t);
}

uint16_t SubmessageHeartbeatGetRawSize(void)
{
  return  SubmessageHeaderGetRawSize() + (2 * 3 + 2 * 1) // EntityID
          + 2 * sizeof(struct SequenceNumber_t) + sizeof(struct Count_t);
}
/*
static const uint16_t SubmessageInfoDSTGetRawSize()
{
  return SubmessageHeaderGetRawSize() + (sizeof(struct GuidPrefix_t));
}
*/
uint16_t getRawSizeWithoutSNSet()
{
  return SubmessageHeaderGetRawSize() + (2 * 3 + 2 * 1) // EntityID
          + sizeof(struct Count_t);
}

uint16_t SubmessageAckNackGetRawSize(const SequenceNumberSet set)
{
  uint16_t bitMapSize = 0;
  if (set.numBits != 0) {
    bitMapSize = 4 * ((set.numBits / 32) + 1);
  }
  return getRawSizeWithoutSNSet() + sizeof(struct SequenceNumber_t) +
          sizeof(uint32_t) + bitMapSize; // SequenceNumberSet
}
